# -*- coding: utf-8 -*-

import numpy as np
import threading
import time
from datetime import datetime
import jderobot
import math
import cv2
from math import pi as pi
import random
from gui.GUI import Particle
from PyQt5 import QtGui
import copy
from jderobotTypes import CMDVel

time_cycle = 10
        

class MyAlgorithm(threading.Thread):

    def __init__(self, pose3d, motors, laser, map_img):
        # Robot's sensors and actuators
        # ------------------
        self.pose3d = pose3d
        self.motors = motors
        self.laser = laser
        # ------------------
        self.map_img = map_img

        self.particleClicked = None
        
        self.stop_event = threading.Event()
        self.kill_event = threading.Event()
        self.lock = threading.Lock()
        threading.Thread.__init__(self, args=self.stop_event)

    def run (self):
        while (not self.kill_event.is_set()):
           
            start_time = datetime.now()

            if not self.stop_event.is_set():
                self.execute()

            finish_Time = datetime.now()

            dt = finish_Time - start_time
            ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
            #print (ms)
            if (ms < time_cycle):
                time.sleep((time_cycle - ms) / 1000.0)

    def stop (self):
        self.stop_event.set()

    def play (self):
        if self.is_alive():
            self.stop_event.clear()
        else:
            self.start()

    def kill (self):
        self.kill_event.set()

    def parse_laser_data(self,laser_data):
        laser = []
        for i in range(180):
            dist = laser_data.values[i]
            angle = math.radians(i)
            laser += [(dist, angle)]
        return laser

    def setParticles (self, particles):
        gui = self.gui
        gui.setParticles(particles)

    def setEstimation (self, est):
        gui = self.gui
        gui.setEstimation(est)

    def paintTheoricalLaser(self, laser):
        self.gui.map1.tLaser = laser

    def execute(self):

        # TODO
        # Add your code here
        
